#include <stdio.h>

#include "../log.h"
#include "../memmap.h"

#include "display.h"
#include "dlint.h"
#include "ge.h"
#include "gpu.h"
#include "texture.h"

#include "vertexdec.h"

const s32 tcsize[4] = {0, 2, 4, 8};
const s32 tcalign[4] = {0, 1, 2, 4};

const s32 colsize[8] = {0, 0, 0, 0, 2, 2, 2, 4};
const s32 colalign[8] = {0, 0, 0, 0, 2, 2, 2, 4};

const s32 nrmsize[4] = {0, 3, 6, 12};
const s32 nrmalign[4] = {0, 1, 2, 4};

const s32 possize[4] = {0, 3, 6, 12};
const s32 posalign[4] = {0, 1, 2, 4};

const s32 wtsize[4] = {0, 1, 2, 4};
const s32 wtalign[4] = {0, 1, 2, 4};

s32 align(s32 n, s32 align)
{
    return (n + (align - 1)) & ~(align - 1);
}

void vertexdec_setVertexType(VertexDecoder *vd, u32 fmt)
{
    s32 biggest = 0;
    
    vd->fmt = fmt;
    vd->size = 0;

    vd->tc          =  (fmt & GE_VTYPE_TC_MASK     ) >> GE_VTYPE_TC_SHIFT      ;
    vd->col         =  (fmt & GE_VTYPE_COL_MASK    ) >> GE_VTYPE_COL_SHIFT     ;
    vd->nrm         =  (fmt & GE_VTYPE_NRM_MASK    ) >> GE_VTYPE_NRM_SHIFT     ;
    vd->pos         =  (fmt & GE_VTYPE_POS_MASK    ) >> GE_VTYPE_POS_SHIFT     ;
    vd->weighttype  =  (fmt & GE_VTYPE_WEIGHT_MASK ) >> GE_VTYPE_WEIGHT_SHIFT  ;
    vd->idx         =  (fmt & GE_VTYPE_IDX_MASK    ) >> GE_VTYPE_IDX_SHIFT     ;
    vd->nweights    = ((fmt & GE_VTYPE_NWEIGHT_MASK) >> GE_VTYPE_NWEIGHT_SHIFT) + 1;
    vd->morphcount  = ((fmt & GE_VTYPE_NMORPH_MASK ) >> GE_VTYPE_NMORPH_SHIFT ) + 1;
    vd->throughmode =  (fmt & GE_VTYPE_THROUGH_MASK) >> GE_VTYPE_THROUGH_SHIFT ;

    _log(TRC, GFX, "VTYPE: THRU = %d TC = %d COL = %d POS = %d NRM = %d WT = %d NW = %d IDX = %d MC = %d",
            vd->throughmode, vd->tc, vd->col, vd->pos, vd->nrm, vd->weighttype, vd->nweights, vd->idx, vd->morphcount);

    if (vd->morphcount == 1)
        gstate.morphWeights[0] = 1.0f;

    if (vd->weighttype)
    {
        vd->size += wtsize[vd->weighttype] * vd->nweights;
        if (wtalign[vd->weighttype] > biggest)
            biggest = wtalign[vd->weighttype];
    }

    if (vd->tc)
    {
        vd->size = align(vd->size, tcalign[vd->tc]);
        vd->tcoff = vd->size;
        vd->size += tcsize[vd->tc];
        if (tcalign[vd->tc] > biggest)
            biggest = tcalign[vd->tc];
    }

    if (vd->col)
    {
        vd->size = align(vd->size, colalign[vd->col]);
        vd->coloff = vd->size;
        vd->size += colsize[vd->col];
        if (colalign[vd->col] > biggest)
            biggest = colalign[vd->col]; 
    }

    if (vd->nrm)
    {
        vd->size = align(vd->size, nrmalign[vd->nrm]);
        vd->nrmoff = vd->size;
        vd->size += nrmsize[vd->nrm];
        if (nrmalign[vd->nrm] > biggest)
            biggest = nrmalign[vd->nrm]; 
    }

    /* Size always exist */
    vd->size = align(vd->size, posalign[vd->pos]);
    vd->posoff = vd->size;
    vd->size += possize[vd->pos];
    if (posalign[vd->pos] > biggest)
        biggest = posalign[vd->pos];

    vd->size = align(vd->size, biggest);
    vd->oneSize = vd->size;
    vd->size *= vd->morphcount;
    _log(TRC, GFX, "SVT : size = %d, aligned to biggest %d", vd->size, biggest);
}

void vertex_decode(VertexDecoder *vd, PtrVertex *vert, u32 vertsAddr, u32 indsAddr, u32 cur)
{
    u32 j, n, index, curAddr;
    
    switch (vd->idx)
    {
    case GE_VTYPE_IDX_8BIT:
        index = mem_read8(indsAddr + cur);
        break;

    case GE_VTYPE_IDX_16BIT:
        index = mem_read16(indsAddr + cur * 2);
        break;

    default:
        index = cur;
        break;
    }

    curAddr = vertsAddr + (index * vd->size);

    switch (vd->weighttype)
    {
    case GE_VTYPE_WEIGHT_NONE:
        break;

    case GE_VTYPE_WEIGHT_8BIT:
        for (j = 0; j < vd->nweights; j++)
            vert->wt[j] = (float)mem_read8(curAddr + j) / 255.0f;
        break;

    case GE_VTYPE_WEIGHT_16BIT:
        for (j = 0; j < vd->nweights; j++)
            vert->wt[j] = (float)mem_read16(curAddr + j * 2) / 65535.0f;
        break;

    case GE_VTYPE_WEIGHT_FLOAT:
        for (j = 0; j < vd->nweights; j++)
            vert->wt[j] = mem_readFloat(curAddr + j * 4);
        break;
    }

    switch (vd->tc)
    {
    case GE_VTYPE_TC_NONE:
        vert->uv[0] = 0.0f;
        vert->uv[1] = 0.0f;
        break;

    case GE_VTYPE_TC_8BIT:
        vert->uv[0] = (float)mem_read16(curAddr + vd->tcoff + 0) / 255.0f;
        vert->uv[1] = (float)mem_read16(curAddr + vd->tcoff + 1) / 255.0f;
        break;

    case GE_VTYPE_TC_16BIT:
        if (vd->throughmode) {
            vert->uv[0] = (float)mem_read16(curAddr + vd->tcoff + 0) / (float)gstate.texWidth[0];
            vert->uv[1] = (float)mem_read16(curAddr + vd->tcoff + 2) / (float)gstate.texHeight[0];
        }
        else {
            vert->uv[0] = (float)mem_read16(curAddr + vd->tcoff + 0) / 65535.0f;
            vert->uv[1] = (float)mem_read16(curAddr + vd->tcoff + 2) / 65535.0f;
        }
        break;

    case GE_VTYPE_TC_FLOAT:
        if (vd->throughmode) {
            vert->uv[0] = mem_readFloat(curAddr + vd->tcoff + 0) / (float)gstate.texWidth[0];
            vert->uv[1] = mem_readFloat(curAddr + vd->tcoff + 4) / (float)gstate.texHeight[0];
        }
        else {
            vert->uv[0] = mem_readFloat(curAddr + vd->tcoff + 0);
            vert->uv[1] = mem_readFloat(curAddr + vd->tcoff + 4);
        }
        break;
    }

    switch (vd->col)
    {
    case GE_VTYPE_COL_4444:
        {
            u16 cdata = mem_read16(curAddr + vd->coloff);
            for (j = 0; j < 4; j++)
                vert->c[j] = (float)(cdata >> (j * 4) & 0xf) / 15.0f;
        }
        break;

    case GE_VTYPE_COL_565:
        {
            u16 cdata = mem_read16(curAddr + vd->coloff);
            vert->c[0] = (float)((cdata >> 0)  & 0x1f) / 31.0f;
            vert->c[1] = (float)((cdata >> 5)  & 0x3f) / 63.0f;
            vert->c[2] = (float)((cdata >> 11) & 0x1f) / 31.0f;
            vert->c[3] = 1.0f;
        }
        break;

    case GE_VTYPE_COL_5551:
        {
            u16 cdata = mem_read16(curAddr + vd->coloff);
            vert->c[0] = (float)((cdata >> 0)  & 0x1f) / 31.0f;
            vert->c[1] = (float)((cdata >> 5)  & 0x1f) / 31.0f;
            vert->c[2] = (float)((cdata >> 10) & 0x1f) / 31.0f;
            vert->c[3] = (float)((cdata >> 15) & 0xff) /  1.0f;
        }
        break;

    case GE_VTYPE_COL_8888:
        for (j = 0; j < 4; j++)
            vert->c[j] = (float)(u8)mem_read8(curAddr + vd->coloff + j) / 255.0f;
        break;

    default:
        vert->c[0] = vert->c[1] = vert->c[2] = vert->c[3] = 1.0f;
        break;
    }

    for (n = 0; n < vd->morphcount; n++)
    {
        switch (vd->nrm)
        {
        case 0:
            /* no normals */
            memset(vert->normal, 0, sizeof(float) * 3);
            break;

        case GE_VTYPE_NRM_FLOAT:
            for (j = 0; j < 3; j++)
                vert->normal[j] = mem_readFloat(curAddr + vd->nrmoff + vd->oneSize * n + j * 4) * gstate.morphWeights[n];
            break;

        case GE_VTYPE_NRM_16BIT:
            for (j = 0; j < 3; j++)
                vert->normal[j] = (mem_read16(curAddr + vd->nrmoff + vd->oneSize * n + j * 2) / 32767.0f) * gstate.morphWeights[n];
            break;

        default:
            _log(ERR, GFX, "Unknown normal format %d", vd->nrm);
            break;
        }
    }

    if (gstate.reversenormals)
        for (j = 0; j < 3; j++)
            vert->normal[j] = -vert->normal[j];

    memset(vert->v, 0, sizeof(float) * 3);
    for (n = 0; n < vd->morphcount; n++)
    {
        switch (vd->pos)
        {
        case GE_VTYPE_POS_FLOAT:
            for (j = 0; j < 3; j++)
                vert->v[j] += mem_readFloat(curAddr + vd->posoff + vd->oneSize * n + j * 4) * gstate.morphWeights[n];
            break;

        case GE_VTYPE_POS_16BIT:
            for (j = 0; j < 3; j++)
                vert->v[j] = ((float)mem_read16(curAddr + vd->posoff + vd->oneSize * n + j * 2) * gstate.morphWeights[n]) / (vd->throughmode ? 1.0f : 32767.0f);
            break;

        case GE_VTYPE_POS_8BIT:
            for (j = 0; j < 3; j++)
                vert->v[j] = ((float)mem_read8(curAddr + vd->posoff + vd->oneSize * n + j) * gstate.morphWeights[n]) / (vd->throughmode ? 1.0f : 127.0f);
            break;

        default:
            _log(ERR, GFX, "Unknown position format %d", vd->pos);
            break;
        }
    }
    _log(TRC, GFX, "[%03d] Position (%10.5f, %10.5f, %10.5f), Texture (%6.5f, %6.5f), Color (%6.5f, %6.5f, %6.5f, %6.5f)", index, vert->v[0], vert->v[1], vert->v[2], vert->uv[0], vert->uv[1],  vert->c[0], vert->c[1], vert->c[2], vert->c[3]);
}

